#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <assert.h>

#include "heap.h"
#include "bit_reversed_counter.h"

//for better readability, we define the following:
#define TAG(i) (h->data[i].tag)
#define PRIORITY(i) (h->data[i].priority)
#define PID(i) (h->data[i].pid)
#define VALUE(i) (h->data[i].value)
#define LOCK(i) {pthread_mutex_lock(&(h->data[i].lock)); }
#define UNLOCK(i) { pthread_mutex_unlock(&(h->data[i].lock)); }

#define DEBUG(X) 

typedef enum tag{EMPTY = 0, AVAILABLE = 1, PID = 2} tag;


typedef struct node
{
	int tag;
	pthread_t pid;	// care only if tag=PID
	pthread_mutex_t lock;
	int priority;
	int value;
}node;

/**
 * initialize a node
 * @param n the node to initialize
 */
void initNode(node* n)
{
	n->tag = EMPTY;
	n->pid = 0;
	n->priority = 0;
	n->value = 0;
	pthread_mutex_init(&(n->lock), NULL);
}

typedef struct heap_t
{
	int maxSize;
	brc size;
	pthread_mutex_t lock; // lock on the size
	node* data;
	pthread_rwlock_t globalLock;
}heap_t;


/**
 * Swap two ints.
 * @param a the first int
 * @param b the second int
 */
static void swap(int *a, int *b)
{
	int t = *a;
	*a = *b;
	*b = t;
}

/**
 * Create and initialize a heap of the specified size
 * @param maxSize the maximum size of the heap 
 * @return a new and initialized heap
 */
Heap Heap_Create(int maxSize)
{
	Heap h = (Heap)malloc(sizeof(heap_t));
	
	// round maxSize to the next power of 2
	int curr = 4;
	while(curr <= maxSize)
		curr *= 2;
	maxSize = curr;
	
	h->maxSize = maxSize;
	pthread_mutex_init(&(h->lock), NULL);
	pthread_rwlock_init(&(h->globalLock), NULL);
	h->size = bit_reversed_counter_Init();
	h->data = (node*) malloc(sizeof(node) * (maxSize + 1 ));
	//initialize all the heap's nodes
	for(int i = 1; i <= maxSize; i++)
	{
		initNode(&(h->data[i]));
	}
	return h;
}

/**
 * Delete the Heap h
 * @param h the heap to delete
 */
void Heap_Delete(Heap h)
{
	if(!h)
		return;
	free(h->data);
	pthread_mutex_destroy(&(h->lock));
	pthread_rwlock_destroy(&(h->globalLock));
	free(h);
}

/**
 * Swap the nodes number i and j of the heap h
 * @param h the heap on which the function will be executed
 * @param i the number of the node
 * @param j a node
 */ 
void swap_nodes(Heap h, int i, int j)
{
	swap(&(PRIORITY(i)), &(PRIORITY(j)));
	swap(&(VALUE(i)), &(VALUE(j)));
	swap(&(TAG(i)), &(TAG(j)));

	pthread_t tmp = PID(i);
	PID(i) = PID(j);
	PID(j) = tmp;	
}

/**
 * Insert a new node with the secified values to the specified heap
 * @param h the heap on which the function will be executed
 * @param prio the priority of the node to insert
 * @param value the value of the node to insert
 */ 
void Heap_Insert(Heap h, int prio, int value)
{
	DEBUG(printf("thread %d: insert\n", pthread_self()%1000);)
	
	//Do a readLock on the heap
	pthread_rwlock_rdlock(&(h->globalLock));	
		
	//Insert new item at bottom of the heap
	
	//Lock the heap size
	pthread_mutex_lock(&(h->lock));	
	//get the real size of the heap
	//int i = brc_getRealValue(&h->size);
	
	//if the real size of the heap is the max size, double the heap size
	while(h->maxSize == brc_getRealValue(&h->size)) // maybe add margin of 10% or something?
	{
		DEBUG(printf("Approching size problems\n");)
		
		//unlock the heap size
		pthread_mutex_unlock(&(h->lock));
		
		// upgrade to Write Lock
		pthread_rwlock_unlock(&(h->globalLock));
		pthread_rwlock_wrlock(&(h->globalLock));
		
		// in the meantime, it is possible another thread already increased the size
		if(h->maxSize == brc_getRealValue(&h->size)) // maybe add margin of 10% or something?
		{
			int previousSize = h->maxSize;
			int newSize = previousSize * 2;
			
			//DEBUG(printf("doubling the size from %d to %d\n", previousSize, newSize);)
			
			//double the heap size
			h->maxSize = newSize;
			node* newData = (node*) malloc(sizeof(node) * (newSize + 1 ));
			
			DEBUG(
				printf("Now printing data on all nodes - BEFORE\n");
				for(int j = 1 ; j <= previousSize; ++j)
				{
					printf("Node %d: value = %d, priority = %d, TAG = %d, \n",j, VALUE(j), PRIORITY(j), TAG(j));
				}		
				printf("\n\n\n-------------\n\n\n");
			)
			
			//move the old nodes to newData and initialize the new nodes
			for(int j = 1; j <= previousSize; ++j)
			{
				newData[j] = h->data[j];
			}
			for(int j = previousSize + 1; j <= newSize; ++j)
			{
				initNode(&(newData[j]));
			}
			
			//replace data with newData
			free(h->data);
			h->data = newData;

			DEBUG(
				printf("Now trying to lock and unlock everything\n");
				
				for(int i = 1 ; i <= h->maxSize; ++i)
				{
					LOCK(i);
				}
				for(int i = 1 ; i <= h->maxSize; ++i)
				{
					UNLOCK(i);
				}
												
				printf("Now trying to lock and unlock everything - DONE\n");
				
				printf("\n\n\n-------------\n\n\n");
				printf("Now printing data on all nodes - AFTER\n");
				for(int j = 1 ; j <= h->maxSize; ++j)
				{
					printf("Node %d: value = %d, priority = %d, TAG = %d, \n",j, VALUE(j), PRIORITY(j), TAG(j));
				}
				printf("\nDONE\n\n\n");
			)
		}
		
		// downgrade to Read Lock
		pthread_rwlock_unlock(&(h->globalLock));
		pthread_rwlock_rdlock(&(h->globalLock));
		
		//lock the heap size
		pthread_mutex_lock(&(h->lock));

		//i = brc_getRealValue(&h->size);
		//printf("thread %d: real value = %d, maxSize == %d\n", pthread_self()%1000, i, h->maxSize);
	}
	
	//Find the location where to insert the node
	int i = brc_Increment(&h->size);
	
	//Lock that node
	LOCK(i);
	//Unlock the heap size
	pthread_mutex_unlock(&(h->lock));	
	//update the node's values with the values passed as parameters
	PRIORITY(i) = prio;
	VALUE(i) = value;
	TAG(i) = PID;
	PID(i) = pthread_self(); // get current pid
	//Unlock that node
	UNLOCK(i);
	int xx = 0;	
	//Move item towards top of heap while it has lower priority than parent
	while(i > 1)
	{
		//Find the parent of the current node
		int parent = i/2;
		
		//Lock parent and the current node
		LOCK(parent);
		LOCK(i);
	
		
		if(TAG(i) == PID && TAG(parent) == PID && PID(i) == pthread_self() && PID(parent) == pthread_self())
		{
				assert(0);
		}
		
		//If the parent's node is available and the pid of the current node is our pid
		if(TAG(parent) == AVAILABLE && TAG(i) == PID && PID(i) == pthread_self())
		{
			//if the parent node has a higher priority, then swap the nodes
			if(PRIORITY(i) < PRIORITY(parent))
			{
				swap_nodes(h, i, parent);
				//Unlock the two nodes after swap
				UNLOCK(i);
				UNLOCK(parent);
				//continue the "while" loop with the parent value as the new current node
				i = parent;
				continue;
			}
			//the parent node has NOT a higher priority, then make available the current node, unlock the nodes and exit the while loop
			else
			{
				TAG(i) = AVAILABLE;
				UNLOCK(i);
				UNLOCK(parent);
				i = 0;
				break;
			}
		}
		//if the parent's node doesn't contain data
		else if (TAG(parent) == EMPTY)
		{
			UNLOCK(i);
			UNLOCK(parent);
			i = 0;
			break;
		}
		else if (((TAG(i) == PID) && (PID(i) != pthread_self())) || TAG(i) != PID)
		{
			UNLOCK(i);
			UNLOCK(parent);
			i = parent;
			continue;
		}
		UNLOCK(i);
		UNLOCK(parent);
	}
	//If we are at the top of the heap, change the tag's node to available
	if(i == 1)
	{
		LOCK(i);
		if(TAG(i) == PID && PID(i) == pthread_self())
		{
			TAG(i) = AVAILABLE;
		}
		UNLOCK(i);
	}
	
	//Unlock the ReadLock on the heap
	pthread_rwlock_unlock(&(h->globalLock));	
	
	DEBUG(printf("thread %d: end of insert\n", pthread_self()%1000);)
}

/**
 * Remove the root node of the heap h and returns its value
 * @param h The heap on which the function will be executed
 * @param error if not null, will contain 1 in case of success, 0 in case of failure (heap empty, etc...)
 * @param priority if not null, will contain the priority of the heap's root
 * @return the value of the heap's root
 */
int Heap_Pop(Heap h, int* error, int *priority)
{	
	DEBUG(printf("thread %d: pop\n", pthread_self()%1000);)
	//Grab item from bottom of heap to replace "to-be-deleted" top item
	
	//Do a readLock on the heap
	pthread_rwlock_rdlock(&(h->globalLock));
	
	//lock the heap size
	pthread_mutex_lock(&(h->lock));	
	int bottom = brc_Decrement(&h->size);
	
	//if the heap is empty, stop and return -1
	if(bottom == 0)
	{
		if(error != NULL )
		   *error = 1;
		//Unlock the heap size before the function's end
		pthread_mutex_unlock(&(h->lock));
		//unlock the readlock
		pthread_rwlock_unlock(&(h->globalLock));	
		DEBUG(printf("thread %d: end of pop\n", pthread_self()%1000);)
		return -1;
	}
	else if(error != NULL)
	{
		*error = 0; // no error
	}
	LOCK(bottom);
	
	/*In the original algorithm, the unlock on the heap size was here.
	 * Doing this causes bugs when multiples threads are executing POPS on the heap in the same times.
	 * pthread_mutex_unlock(&(h->lock));
	*/
	
	TAG(bottom) = EMPTY;
	//save value and priority of "bottom"
	int prio = PRIORITY(bottom);
	int value = VALUE(bottom);
	
	UNLOCK(bottom);
	
	//Lock first item. Stop if it was the only item in the heap
	LOCK(1);
	/*
	 * Doing the unlock on the heap size here solves the problem
	 */
	pthread_mutex_unlock(&(h->lock));
	
	if( TAG(1) == EMPTY)
	{
		UNLOCK(1);
		
		pthread_rwlock_unlock(&(h->globalLock));	
		
		if(priority)
			*priority = prio;
		
		DEBUG(printf("thread %d: end of pop\n", pthread_self()%1000);)
		DEBUG(printf("returning last: prio = %d\n", prio);)
		
		return value;
	}
	
	//Replace the top item with the item stored from the bottom
	swap(&prio, &(PRIORITY(1)));
	swap(&value, &(VALUE(1)));
	TAG(1) = AVAILABLE;
	//Heapify the heap starting from the root. Always hold lock on item being adjusted
	int i = 1;
	while(i < h->maxSize / 2)
	{
		int left = i*2, right = i*2 + 1;
		// lock both chidren
		LOCK(left); 
		LOCK(right);
		
		// get the child with the least priority
		int child;
		if(TAG(left) == EMPTY) // it means the right child is also empty
		{
			UNLOCK(right);
			UNLOCK(left);
			break;
		}
		else if (TAG(right) == EMPTY || PRIORITY(left) < PRIORITY(right))
		{
			UNLOCK(right);
			child = left;			
		}
		else
		{
			UNLOCK(left);
			child = right;
		}
		
		//If child has lower priority than parent then swap nodes; if not, stop
		if(PRIORITY(child) < PRIORITY(i))
		{
			swap_nodes(h, i, child);
			UNLOCK(i);
			i = child;
		}
		else
		{
			UNLOCK(child);
			break;
		}
	}
	UNLOCK(i);
	
	pthread_rwlock_unlock(&(h->globalLock));	
	
	if(priority)
		*priority = prio;
	
	DEBUG(printf("thread %d: end of pop\n", pthread_self()%1000);)
	
	return value;
}

/**	
 * Return the value of the root node of the heap h
 * @param h The heap on which the function will be executed
 * @param error if not null, will contain 1 in case of success, 0 in case of failure (heap empty, etc...)
 * @return the value of the heap's root
 */
int Heap_GetMin(Heap h, int *error, int *priority)
{
	//Do a readLock on the heap
	pthread_rwlock_rdlock(&(h->globalLock));	
		
	//lock the heap size
	pthread_mutex_lock(&(h->lock));	
	int size = brc_getRealValue(&h->size);
	
	//if the heap is empty, stop and return -1
	if(size == 0)
	{
		if(error != NULL )
		   *error = 1;
		//Unlock the heap size before the function's end
		pthread_mutex_unlock(&(h->lock));	
		return -1;
	}
	else if(error != NULL)
		*error = 0; // no error
	
	LOCK(1);
	pthread_mutex_unlock(&(h->lock));

	int prio = PRIORITY(1);
	int value = VALUE(1);

	UNLOCK(1);
	
	if(priority)
		*priority = prio;
	return value;
	
	pthread_rwlock_unlock(&(h->globalLock));	
}


/**	
 * Return the number of elements currently in the heap.
 * @param h The heap on which the function will be executed
 * @return the number of elements currently in the heap
 */
int Heap_GetSize(Heap h)
{
	pthread_rwlock_rdlock(&(h->globalLock));	
	pthread_mutex_lock(&(h->lock));
	
	int size = brc_getRealValue(&h->size);
	
	pthread_mutex_unlock(&(h->lock));	
	pthread_rwlock_unlock(&(h->globalLock));	
	
	return size;
}


/**
 * For testing purposes: check if the heap is well ordered and display the errors if found
 * @param h the heap to perform the test on
 */
void Heap_Check(Heap h)
{
	pthread_rwlock_wrlock(&(h->globalLock));	
	
	for(int i = 1; i <= h->maxSize; ++i)
	{
		if(TAG(i) == PID)
		{
			printf("ERROR: node %d is not available\n",i);
			continue;
		}
		if(TAG(i) == EMPTY)
		{
			continue;
		}
		
		int p = PRIORITY(i);
		
		if(i*2 <= h->maxSize &&  TAG(i*2) == AVAILABLE && PRIORITY(i*2) < p)
			printf("ERROR: left child of node %d hav priority %d < %d\n", i, p, PRIORITY(i*2));
		if(i*2 +1 <= h->maxSize &&  TAG(i*2 + 1) == AVAILABLE && PRIORITY(i*2 + 1) < p)
			printf("ERROR: right child of node %d hav priority %d < %d\n", i, p, PRIORITY(i*2 + 1));
	}
	
	pthread_rwlock_unlock(&(h->globalLock));
}



